﻿/*
 * This file is part of MonoStrategy.
 *
 * Copyright (C) 2010-2011 Christoph Husse
 *
 *  This program is free software: you can redistribute it and/or modify
 *  it under the terms of the GNU Affero General Public License as
 *  published by the Free Software Foundation, either version 3 of the
 *  License, or (at your option) any later version.
 *
 *  This program is distributed in the hope that it will be useful,
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 *  GNU Affero General Public License for more details.
 *
 *  You should have received a copy of the GNU Affero General Public License
 *  along with this program.  If not, see <http://www.gnu.org/licenses/>.
 *
 * Authors: 
 *      # Christoph Husse
 * 
 * Also checkout our homepage: http://monostrategy.codeplex.com/
 */
using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

namespace MonoStrategy
{

    internal class AStarSolver
    {
        private OpenCloseMap m_ClosedSet;
        private OpenCloseMap m_OpenSet;
        private PriorityQueue<PathFinderNode> m_OrderedOpenSet;
        private PathFinderNode[] m_CameFrom;
        private PathFinderNode[] m_SearchSpace;
        private PathFinderNode m_StartNode;
        private PathFinderNode m_EndNode;
        private Byte[] m_UserGrid;
        private int m_UserContext;
        private int Size;
        private int SizeShift;
        internal TerrainDefinition Terrain { get; private set; }

        internal AStarSolver(TerrainDefinition inTerrain)
        {
            double log2Factor = 1.0 / Math.Log(2.0);
            int x, y;
            PathFinderNode node;

            Terrain = inTerrain;
            Size = inTerrain.Size;
            SizeShift = (int)Math.Floor((Math.Log((double)Size) * log2Factor) + 0.5);

            if ((int)Math.Pow(2, (double)SizeShift) != Size)
                throw new ArgumentException();

            m_ClosedSet = new OpenCloseMap(Size);
            m_OpenSet = new OpenCloseMap(Size);
            m_OrderedOpenSet = new PriorityQueue<PathFinderNode>();
            m_CameFrom = new PathFinderNode[Size * Size];
            m_SearchSpace = new PathFinderNode[Size * Size];

            for (x = 0; x < Size; x++)
            {
                for (y = 0; y < Size; y++)
                {
                    node = m_SearchSpace[x + (y << SizeShift)] = new PathFinderNode();

                    node.X = x;
                    node.Y = y;
                }
            }
        }


        const Double SQRT_2 = 1.41421356;

        internal double Heuristic(Point inStart, Point inEnd)
        {
            return Math.Sqrt((double)((inStart.X - inEnd.X) * (inStart.X - inEnd.X) + (inStart.Y - inEnd.Y) * (inStart.Y - inEnd.Y)));
        }

        double Heuristic(PathFinderNode inStart, PathFinderNode inEnd)
        {
            return Math.Sqrt((double)((inStart.X - inEnd.X) * (inStart.X - inEnd.X) + (inStart.Y - inEnd.Y) * (inStart.Y - inEnd.Y)));
        }

        double NeighborDistance(PathFinderNode inStart, PathFinderNode inEnd)
        {
            int diffX = Math.Abs(inStart.X - inEnd.X);
            int diffY = Math.Abs(inStart.Y - inEnd.Y);

            switch (diffX + diffY)
            {
                case 1: return 1;
                case 2: return SQRT_2;
                default:
                    return 0;
            }
        }

        Point ClipPoint(Point inPoint)
        {
            if (inPoint.X < 0)
                inPoint.X = 0;

            if (inPoint.Y < 0)
                inPoint.Y = 0;

            if (inPoint.X > Size - 1)
                inPoint.X = Size - 1;

            if (inPoint.Y > Size - 1)
                inPoint.Y = Size - 1;

            return inPoint;
        }

        internal void BeginSearch(Point inStartNode, Point inEndNode, int inUserContext)
        {
            Point startNode = ClipPoint(inStartNode);
            Point endNode = ClipPoint(inEndNode);

            m_StartNode = m_SearchSpace[startNode.X + (startNode.Y << SizeShift)];
            m_EndNode = m_SearchSpace[endNode.X + (endNode.Y << SizeShift)];
            m_UserContext = inUserContext;
            m_UserGrid = Terrain.GetWallMap();

            m_ClosedSet.Clear();
            m_OpenSet.Clear();
            m_OrderedOpenSet.Clear();

            Array.Clear(m_CameFrom, 0, m_CameFrom.Length);

            m_StartNode.G = 0;
            m_StartNode.H = Heuristic(m_StartNode, m_EndNode);
            m_StartNode.F = m_StartNode.H;

            if (m_UserGrid[m_StartNode.X + (m_StartNode.Y << SizeShift)] > m_UserContext)
                return; // if we can't walk on the start node we can't walk at all...

            m_OpenSet.Add(m_StartNode);
            m_OrderedOpenSet.Push(m_StartNode);
        }

        internal double GetClosedGValue(Point inForNode)
        {
            inForNode = ClipPoint(inForNode);

            SearchLoop(inForNode);

            if (!m_ClosedSet.IsSet(inForNode))
                return -1;

            return m_ClosedSet[inForNode].G;
        }

        internal void SearchLoop(Point inRequiredClosedNode)
        {
            SearchLoop(inRequiredClosedNode, Int32.MaxValue / 2, null);
        }

        internal void ComputeConnectedTileAround(Point inAround, int inUserContext, List<Point> outConnectedTileAround)
        {
            BeginSearch(inAround, inAround, inUserContext);

            outConnectedTileAround.Add(inAround);

            SearchLoop(null, Int32.MaxValue / 2, outConnectedTileAround);
        }

        internal void ComputePath(Point inStart, Point inEnd, int inUserContext, List<Point> outPathNodes)
        {
            BeginSearch(inStart, inEnd, inUserContext);

            if (GetClosedGValue(inEnd) < 0)
                throw new ArgumentException("No path could be found from [" + inStart + "] to [" + inEnd + "]!");

            PathFinderNode node = m_CameFrom[inEnd.X + (inEnd.Y << SizeShift)];

            outPathNodes.Add(inEnd);

            while ((node.X != inStart.X) || (node.Y != inStart.Y))
            {
                outPathNodes.Insert(0, new Point(node.X, node.Y));

                node = m_CameFrom[node.X + (node.Y << SizeShift)];
            }

            outPathNodes.Add(inStart);
        }

        private void SearchLoop(Point? inRequiredClosedNode, Int32 inMaxRadius, List<Point> outConnectedTileAround)
        {
            double tentative_g_score;
            bool wasAdded = false;
            PathFinderNode[] neighborNodes = new PathFinderNode[8];
            PathFinderNode y;
            PathFinderNode x;
            bool tentative_is_better;
            int i;
            Point reqCloseNode = new Point(0, 0);
            bool hasReqCloseNode = (inRequiredClosedNode != null) && inRequiredClosedNode.HasValue;

            if(hasReqCloseNode)
                reqCloseNode = ClipPoint(inRequiredClosedNode.Value);

            if (hasReqCloseNode && m_ClosedSet.IsSet(reqCloseNode))
                return;

            while (!m_OpenSet.IsEmpty)
            {
                if (hasReqCloseNode && m_ClosedSet.IsSet(reqCloseNode))
                    return;

                x = m_OrderedOpenSet.Pop();

                m_OpenSet.Remove(x);
                m_ClosedSet.Add(x);

                if ((Math.Abs(x.X - m_StartNode.X) > inMaxRadius) || (Math.Abs(x.Y - m_StartNode.Y) > inMaxRadius))
                    break;

                StoreNeighborNodes(x, neighborNodes);

                for (i = 0; i < 8; i++)
                {
                    y = neighborNodes[i];

                    if (y == null)
                        continue;

                    if (m_UserGrid[y.X + (y.Y << SizeShift)] > m_UserContext)
                        m_ClosedSet.Add(y);

                    if (m_ClosedSet.IsSet(y))
                        continue;

                    tentative_g_score = x.G + NeighborDistance(x, y);
                    wasAdded = false;

                    if (!m_OpenSet.IsSet(y))
                    {
                        m_OpenSet.Add(y);
                        tentative_is_better = true;
                        wasAdded = true;

                        if (outConnectedTileAround != null)
                            outConnectedTileAround.Add(new Point(y.X, y.Y));
                    }
                    else if (tentative_g_score < y.G)
                    {
                        tentative_is_better = true;
                    }
                    else
                    {
                        tentative_is_better = false;
                    }

                    if (tentative_is_better)
                    {
                        m_CameFrom[y.X + (y.Y << SizeShift)] = x;

                        y.G = tentative_g_score;
                        y.H = Heuristic(y, m_EndNode);
                        y.F = y.G + y.H;

                        if (wasAdded)
                            m_OrderedOpenSet.Push(y);
                        else
                            m_OrderedOpenSet.Update(y);
                    }
                }
            }
        }


        void StoreNeighborNodes(PathFinderNode inAround, PathFinderNode[] inNeighbors)
        {
            int x = inAround.X;
            int y = inAround.Y;

            for (int i = 0; i < inNeighbors.Length; i++)
            {
                inNeighbors[i] = null;
            }

            if (!((x > 0) && (y > 0))) { }
            else
                inNeighbors[0] = m_SearchSpace[x - 1 + ((y - 1) << SizeShift)];

            //if (!(y > 0)) { }
            //else
            //    inNeighbors[1] = m_SearchSpace[x + ((y - 1) << SizeShift)];

            if (!((x < Size - 1) && (y > 0))) { }
            else
                inNeighbors[2] = m_SearchSpace[x + 1 + ((y - 1) << SizeShift)];

            if (!(x > 0)) { }
            else
                inNeighbors[3] = m_SearchSpace[x - 1 + (y << SizeShift)];

            if (!(x < Size - 1)) { }
            else
                inNeighbors[4] = m_SearchSpace[x + 1 + (y << SizeShift)];

            if (!((x > 0) && (y < Size - 1))) { }
            else
                inNeighbors[5] = m_SearchSpace[x - 1 + ((y + 1) << SizeShift)];

            //if (!(y < Size - 1)) { }
            //else
            //    inNeighbors[6] = m_SearchSpace[x + ((y + 1) << SizeShift)];

            if (!((x < Size - 1) && (y < Size - 1))) { }
            else
                inNeighbors[7] = m_SearchSpace[x + 1 + ((y + 1) << SizeShift)];
        }

        private class PathFinderNode : IQueueItem
        {
            public double F { get; set; }
            internal double H;
            internal double G;
            internal int X;
            internal int Y;
            public int Index { get; set; }
        }

        private class OpenCloseMap
        {
            private PathFinderNode[] Map;
            private int Width;
            private int Height;
            private int Count;
            private int SizeShift;

            internal PathFinderNode this[int x, int y]
            {
                get { return Map[x + (y << SizeShift)]; }
                set { Map[x + (y << SizeShift)] = value; }
            }

            internal PathFinderNode this[Point point]
            {
                get { return Map[point.X + (point.Y << SizeShift)]; }
                set { Map[point.X + (point.Y << SizeShift)] = value; }
            }


            internal bool IsEmpty
            {
                get
                {
                    return Count == 0;
                }
            }

            internal OpenCloseMap(int inSize)
            {
                double log2Factor = 1.0 / Math.Log(2.0);

                SizeShift = (int)Math.Floor((Math.Log((double)inSize) * log2Factor) + 0.5);

                if ((int)Math.Pow(2, (double)SizeShift) != inSize)
                    throw new ArgumentException();

                Map = new PathFinderNode[inSize * inSize];
                Width = inSize;
                Height = inSize;
            }

            internal void Add(PathFinderNode inValue)
            {
                Count++;
                this[inValue.X, inValue.Y] = inValue;
            }

            internal bool IsSet(Point inValue)
            {
                return this[inValue.X, inValue.Y] != null;
            }

            internal bool IsSet(PathFinderNode inValue)
            {
                return this[inValue.X, inValue.Y] != null;
            }

            internal void Remove(PathFinderNode inValue)
            {
                Count--;
                this[inValue.X, inValue.Y] = null;
            }

            internal void Clear()
            {
                Count = 0;
                Array.Clear(Map, 0, Map.Length);
            }
        }
    }
}
